
#include "Perus.h"


void VEKTORI::NollaVektoriksi()
{
    X = 0.0;
    Y = 0.0;
    Z = 0.0;
}

void VEKTORI::Versoriksi()
{
    double NykyinenPituus = sqrt( X*X + Y*Y + Z*Z);
    
    if(NykyinenPituus > 0)
    {
        X *= 1.0/NykyinenPituus;
        Y *= 1.0/NykyinenPituus;
        Z *= 1.0/NykyinenPituus;
    }
}

void VEKTORI::AsetaPituus( double UusiPituus )
{
    double NykyinenPituus = sqrt( X*X + Y*Y + Z*Z);
    
    if(NykyinenPituus > 0)
    {
        X *= UusiPituus/NykyinenPituus;
        Y *= UusiPituus/NykyinenPituus;
        Z *= UusiPituus/NykyinenPituus;
    }
}


void VEKTORI::Lisaa( VEKTORI LisausV )
{
    X += LisausV.X;
    Y += LisausV.Y;
    Z += LisausV.Z;
}


double VEKTORI::AnnaPituus()
{
    return sqrt( X*X + Y*Y + Z*Z);
}


void VEKTORI::Kaanna( ASENTO DeltaAsento )
{  
    ASENTO Asento;
    
    Asennoksi( Asento );
    
    Asento.Alfa += DeltaAsento.Alfa;
    Asento.Beta += DeltaAsento.Beta;
    Asento.Gamma += DeltaAsento.Gamma;
    
    AsentoVektoriksi( Asento );
    
}

void VEKTORI::AsentoVektoriksi( ASENTO Asento )
{
    AsteistaRadiaaniksi(Asento.Alfa);
    AsteistaRadiaaniksi(Asento.Beta);
    
    double Pituus = sqrt( X*X + Y*Y + Z*Z);
    
    Y = sin(Asento.Beta) * Pituus;
    X = sin(Asento.Alfa) * cos(-Asento.Beta) * Pituus;
    Z = cos(Asento.Alfa) * cos(-Asento.Beta) * Pituus;
}

void VEKTORI::AsentoVersoriksi( ASENTO Asento )
{
    AsteistaRadiaaniksi(Asento.Alfa);
    AsteistaRadiaaniksi(Asento.Beta);
    
    Y = sin(Asento.Beta);
    X = sin(Asento.Alfa) * cos(-Asento.Beta);
    Z = cos(Asento.Alfa) * cos(-Asento.Beta);
}


void VEKTORI::KulmatVektoriksi(double Alfa, double Beta)
{
    AsteistaRadiaaniksi(Alfa);
    AsteistaRadiaaniksi(Beta);
    
    double Pituus = sqrt( X*X + Y*Y + Z*Z);
    
    Y = sin(Beta) * Pituus;
    X = sin(Alfa) * cos(-Beta) * Pituus;
    Z = cos(Alfa) * cos(-Beta) * Pituus;
}

void VEKTORI::KulmatVersoriksi(double Alfa, double Beta)
{
    AsteistaRadiaaniksi(Alfa);
    AsteistaRadiaaniksi(Beta);
    
    Y = sin(Beta);
    X = sin(Alfa) * cos(-Beta);
    Z = cos(Alfa) * cos(-Beta);
}

void VEKTORI::Asennoksi( ASENTO &Asento )
{
    double Pituus = sqrt( X*X + Y*Y + Z*Z);
    
    Versoriksi();

    Asento.Beta = asin( Y );
    Asento.Alfa = acos( X / cos( Asento.Beta ) );
    if(cos( Asento.Beta )<0.001)Asento.Alfa = 0;

    if(Z < 0)Asento.Alfa = -Asento.Alfa;

    RadiaanistaAsteiksi( Asento.Alfa );
    RadiaanistaAsteiksi( Asento.Beta );
    
    //muutos versorista takaisin entiseen pituuteen
    X *= Pituus;
    Y *= Pituus;
    Z *= Pituus;
}

void VEKTORI::Kulmiksi( double &Alfa, double &Beta )
{
    double Pituus = sqrt( X*X + Y*Y + Z*Z);
    
    Versoriksi();

    Beta = asin( Y );
    Alfa = acos( X / cos( Beta ) );
    
    if(Z < 0)Alfa = -Alfa;

    RadiaanistaAsteiksi( Alfa );
    RadiaanistaAsteiksi( Beta );
    
    //muutos versorista takaisin entiseen pituuteen
    X *= Pituus;
    Y *= Pituus;
    Z *= Pituus;
}

void VEKTORI::Aseta( double UusiX, double UusiY, double UusiZ)
{
    X = UusiX;
    Y = UusiY;
    Z = UusiZ;
}

void VEKTORI::KerroLuvulla( double Luku)
{
    X *= Luku;
    Y *= Luku;    
    Z *= Luku;    
}



double VektorienEriavyys( VEKTORI V1, VEKTORI V2 )
{
    V1.Versoriksi();
    V2.Versoriksi();
    double EriavyysX = V1.X - V2.X;
    double EriavyysY = V1.Y - V2.Y;
    double EriavyysZ = V1.Z - V2.Z;
    
    double KokonaisEriavyys = sqrt( EriavyysX*EriavyysX + EriavyysY*EriavyysY + EriavyysZ*EriavyysZ );
    
    return (KokonaisEriavyys * 50.0); //prosenteiksi...
}


double Pistetulo( VEKTORI V1, VEKTORI V2 )
{
    return ( V1.X*V2.X + V1.Y*V2.Y + V1.Z*V2.Z );
}

VEKTORI Vektoritulo( VEKTORI V1, VEKTORI V2 )
{
    VEKTORI PalautusV;
    
    PalautusV.X = V1.Y*V2.Z - V1.Z*V2.Y;
    PalautusV.Y = V1.Z*V2.X - V1.X*V2.Z;
    PalautusV.Z = V1.X*V2.Y - V1.Y*V2.X;
    
    return PalautusV;
}



void SIJAINTI::Aseta( double UusiX, double UusiY, double UusiZ)
{
    X = UusiX;
    Y = UusiY;
    Z = UusiZ;
}

double Etaisyys( SIJAINTI S1, SIJAINTI S2 )
{
    return sqrt( (S1.X - S2.X)*(S1.X - S2.X) + (S1.Y - S2.Y)*(S1.Y - S2.Y) + (S1.Z - S2.Z)*(S1.Z - S2.Z) );
}




/*
    0,  1,   2,  3,
    4,  5,   6,  7,
    8,  9,  10, 11,
    12, 13, 14, 15

    1, 2, 3,
    3, 4, 5,
    6, 7, 8
*/


void MATRIISI4X4::KerroMatriisi( double M1[16], double M2[16] )
{
    M[0] = M1[0]*M2[0] + M1[1]*M2[4] + M1[2]*M2[8];
    M[1] = M1[0]*M2[1] + M1[1]*M2[5] + M1[2]*M2[9];
    M[2] = M1[0]*M2[2] + M1[1]*M2[6] + M1[2]*M2[10];

    M[4] = M1[4]*M2[0] + M1[5]*M2[4] + M1[6]*M2[8];
    M[5] = M1[4]*M2[1] + M1[5]*M2[5] + M1[6]*M2[9];
    M[6] = M1[4]*M2[2] + M1[5]*M2[6] + M1[6]*M2[10];

    M[8] = M1[8]*M2[0] + M1[9]*M2[4] + M1[10]*M2[8];
    M[9] = M1[8]*M2[1] + M1[9]*M2[5] + M1[10]*M2[9];
    M[10]= M1[8]*M2[2] + M1[9]*M2[6] + M1[10]*M2[10];
}


void MATRIISI4X4::PyoritaV( double Kulma, VEKTORI PyoritusAkseli)
{
    AsteistaRadiaaniksi( Kulma );
    double KulmanCos = cos(Kulma);
    double KulmanSin = sin(Kulma);
    double  U = PyoritusAkseli.X, V = PyoritusAkseli.Y, W = PyoritusAkseli.Z;
    
    double MuutosM[16] = {    KulmanCos+U*U*(1.0-KulmanCos),  -W*KulmanSin+U*V*(1.0-KulmanCos),  V*KulmanSin+U*W*(1.0-KulmanCos),  0,
                            W*KulmanSin+V*U*(1.0-KulmanCos),     KulmanCos+V*V*(1.0-KulmanCos), -U*KulmanSin+V*W*(1.0-KulmanCos),  0,
                           -V*KulmanSin+W*U*(1.0-KulmanCos),   U*KulmanSin+W*V*(1.0-KulmanCos),    KulmanCos+W*W*(1.0-KulmanCos),  0,
                                                          0,                                 0,                                0,  1 };

    //RadiaanistaAsteiksi( Kulma );
    Kulma = 0;
    KerroMatriisi( MuutosM, M );
    
}

void MATRIISI4X4::PyoritaX( double Alfa )
{
    AsteistaRadiaaniksi( Alfa );
    double MuutosM[16] = { 1.0,          0,          0,    0,
                           0,  cos(Alfa),  -sin(Alfa),   0,
                           0,  sin(Alfa),   cos(Alfa),   0,
                           0,          0,           0,   1.0 };
    //RadiaanistaAsteiksi( Alfa );
    Alfa = 0;

    KerroMatriisi( MuutosM, M );
}

void MATRIISI4X4::PyoritaY( double Beta )
{
    AsteistaRadiaaniksi( Beta );
    double MuutosM[16] = { cos(Beta),  0,  sin(Beta),   0,
                                   0,  1.0,          0,   0,
                          -sin(Beta),  0,  cos(Beta),   0,
                                   0,  0,          0,   1.0 };
    //RadiaanistaAsteiksi( Beta );
    Beta = 0;
    KerroMatriisi( MuutosM, M );
}



void MATRIISI4X4::PyoritaZ( double Gamma )
{
    AsteistaRadiaaniksi( Gamma );
    double MuutosM[16] = { cos(Gamma), -sin(Gamma),   0,  0,
                           sin(Gamma),  cos(Gamma),   0,  0,
                                    0,           0,   1.0,  0,
                                    0,           0,   0,  1.0 };
    //RadiaanistaAsteiksi( Gamma );
    Gamma = 0;
    KerroMatriisi( MuutosM, M );
}                   


VEKTORI MATRIISI4X4::OikealleV()
{
    VEKTORI OikealleV = { M[0], M[1], M[2] };
    return OikealleV;
}

VEKTORI MATRIISI4X4::YlosV()
{
    VEKTORI YlosV = { M[4], M[5], M[6] };
    return YlosV;
}
                      
VEKTORI MATRIISI4X4::EteenV()
{
    VEKTORI EteenV = { M[8], M[9], M[10] };
    return EteenV;
}

void MATRIISI4X4::AsetaOikealleV( VEKTORI UusiAkseliV )
{
    M[0] = UusiAkseliV.X;
    M[1] = UusiAkseliV.Y;
    M[2] = UusiAkseliV.Z;

}

void MATRIISI4X4::AsetaEteenV( VEKTORI UusiAkseliV )
{
    M[4] = UusiAkseliV.X;
    M[5] = UusiAkseliV.Y;
    M[6] = UusiAkseliV.Z;
}

void MATRIISI4X4::AsetaYlosV( VEKTORI UusiAkseliV )
{
    M[8] = UusiAkseliV.X;
    M[9] = UusiAkseliV.Y;
    M[10] = UusiAkseliV.Z;
}


void MATRIISI4X4::Aseta( const double UusiM[16] )
{
    for(int i=0; i<16; i++)
    {
        M[i] = UusiM[i];
    }
}

void MATRIISI4X4::Orthonormalisoi()
{ 
    VEKTORI V1 = { M[0], M[1], M[2]}, V2 = { M[4], M[5], M[6]}, V3 = { M[8], M[9], M[10]};
    
    V1.Versoriksi();
    
    V3 = Vektoritulo( V1, V2);
    
    V3.Versoriksi();
    
    V2 = Vektoritulo( V3, V1);
    
    V2.Versoriksi();
}


void OrthonormalizeMatrix(double m[16])
{
    NormalizeVector3f(m);
  
    CrossProduct3f(&m[8], m, &m[4]);

    NormalizeVector3f(&m[8]);

    CrossProduct3f(&m[4], &m[8], m);

    NormalizeVector3f(&m[4]);
}

void CrossProduct3f(double result[3], const double v1[3], const double v2[3])
{
    result[0]=v1[1]*v2[2]-v1[2]*v2[1];
    result[1]=v1[2]*v2[0]-v1[0]*v2[2];
    result[2]=v1[0]*v2[1]-v1[1]*v2[0];
}

void NormalizeVector3f(double v[3])
{
    float length;
    length=sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
    if ( length>0 )
    {
        v[0]/=length;
        v[1]/=length;
        v[2]/=length;
    }
}








bool OnkoSisalla( double TarkasteltavaX, double TarkasteltavaY, double X, double Y, double Leveys, double Korkeus )
{
    if(TarkasteltavaX > X && TarkasteltavaX < (X+Leveys) 
    && TarkasteltavaY > Y && TarkasteltavaY < (Y+Korkeus))
        return true;
    
    else return false;
}




void AsteistaRadiaaniksi(double &Kulma)
{
    Kulma = Kulma/360.0 * 2.0*Pii;
}
void RadiaanistaAsteiksi(double &Kulma)
{
    Kulma = Kulma/(2.0*Pii) * 360.0;
}
